﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WinRos;

namespace WinRos
{
    class WinRosTester
    {
        static void Main(string[] args)
        {
            bool quit = false;
            double origXF = 0.1;
            double origZC = -0.5;
            double origZCC = 0.5;
            double origXB = -0.1;
            double x = 0.0;
            double z = 0.0;
            String takeoff = "{\"receiver\":\"/ardrone/takeoff\",\"msg\":{},\"type\":\"std_msgs/Empty\"}";
            String land = "{\"receiver\":\"/ardrone/land\",\"msg\":{},\"type\":\"std_msgs/Empty\"}";

            ConsoleKeyInfo ch = new ConsoleKeyInfo();

            String host = "";
            Console.WriteLine("\t\tWelcome to the WinRos Interface!\n");
            Console.WriteLine("Enter in the IP address of the host server you want to connect to");
            Console.WriteLine("or just press enter to use the defualt server");

            host = Console.ReadLine();
            WinRos ros = new WinRos(host);

            Console.WriteLine("Congrats you have succesfully connected to Rosbridge");

            Console.WriteLine("In order to control the iRobot_Create or Drone use the following keys");
            Console.WriteLine("Note that in order to control a drone you must press N to takeoff. You may land with M");
            Console.WriteLine("------------------------------------------------------------");
            Console.WriteLine("N - Drone takeoff");
            Console.WriteLine("M - Drone Land");
            Console.WriteLine("W - Forward");
            Console.WriteLine("A - Spin CounterClockwise");
            Console.WriteLine("D - Spin Clockwise");
            Console.WriteLine("X - Backward");
            Console.WriteLine("S - Stop");
            Console.WriteLine("I/O - Increase/decrease linear speed by 10%");
            Console.WriteLine("K/L - Increase/decrease angular speed by 10%");
            Console.WriteLine("Q - Quit");

            while (quit == false)
            {
                ch = Console.ReadKey();
                if (ch.KeyChar == 'W' || ch.KeyChar == 'w')
                {
                    //stuff to move forward
                    x = origXF;
                }
                else if (ch.KeyChar == 'A' || ch.KeyChar == 'a')
                {
                    //move counterclockwise
                    z = origZCC;
                }
                else if (ch.KeyChar == 'S' || ch.KeyChar == 's')
                {
                    //stop
                    x = 0.0;
                    z = 0.0;
                }
                else if (ch.KeyChar == 'D' || ch.KeyChar == 'd')
                {
                    //move clockwise
                    z = origZC;
                }
                else if (ch.KeyChar == 'X' || ch.KeyChar == 'x')
                {
                    //move clockwise
                    x = origXB;
                }
                else if (ch.KeyChar == 'I' || ch.KeyChar == 'i')
                {
                    //increase linear speed by ten percent
                    origXB *= 1.10;
                    origXF *= 1.10;
                }
                else if (ch.KeyChar == 'O' || ch.KeyChar == 'o')
                {
                    //decrease linear speed by ten percent
                    origXB *= 0.9;
                    origXF *= 0.9;
                    if (x <= 0.0){x = 0.0;}
                }
                else if (ch.KeyChar == 'K' || ch.KeyChar == 'k')
                {
                    //increase angular speed by ten percent
                    origZC *= 1.10;
                    origZCC *= 1.10;
                }
                else if (ch.KeyChar == 'L' || ch.KeyChar == 'l')
                {
                    //decrease the angular speed by ten percent
                    origZC *= 0.9;
                    origZCC *= 0.9;

                    if (z <= 0.0){z = 0.0;}
                }

                else if (ch.KeyChar == 'N' || ch.KeyChar == 'n')
                {
                    //the ardrone should takeoff
                    ros.publish(takeoff);

                }

                else if (ch.KeyChar == 'M' || ch.KeyChar == 'm')
                {
                    //the ardrone should land
                    ros.publish(land);
                }
                else if (ch.KeyChar == 'Q' || ch.KeyChar == 'q')
                {
                    quit = true;
                    String endMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":0.0}},\"type\":\"geometry_msgs/Twist\"}";
                    ros.publish(land); //just in case you've forgotten to land it
                    ros.publish(endMessage);
                    ros.disconnect();
                    Console.WriteLine("Goodbye!");
                    Environment.Exit(0);
                }
   
                String theMessage = "{\"receiver\":\"/cmd_vel\",\"msg\":{\"linear\":{\"x\":" + x + ",\"y\":0.0,\"z\":0.0}, \"angular\":{\"x\":0.0,\"y\":0.0,\"z\":" + z + "}},\"type\":\"geometry_msgs/Twist\"}";
                ros.publish(theMessage);
            }
            
        }
    }
}
